<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0 Strict//EN">
<html>

<head>
<meta http-equiv="Content-Language" content="en-us">
<title>Inverse kinematics tutorial</title>
<link rel="stylesheet" type="text/css" href="../style.css">
</head>

<body>

<div align="center">
<table class=allEncompassingTable >
 <tr>
  <td >
<p><a href="../index.html" TARGET="_top"><img src="images/homeImg.png"></a></p>



<h1>Inverse kinematics tutorial</h1>

<p>This tutorial explains how to use CoppeliaSim's <a href="kinematics.htm">kinematics functionality</a>, while building a 7 DoF redundant manipulator. But before that, make sure  to have a look at the various example scenes related to IK and FK in folder <em>scenes/kinematics</em>. </p>
<p>This tutorial is segmented into 3 parts:</p>
<li><a href="#model">Building</a> the simple simulation model of a redundant manipulator</li>
<li><a href="#setup">Setting up</a> an inverse kinematics task</li>
<li><a href="#run">Testing</a> the inverse kinematics</li>

<br>
<br>
<table class=subsectionTable><tr class=subsectionTd><td class=subsectionTd>
<a name="model"></a>Simple simulation model
</td></tr></table>

<p>For this tutorial, we will build a non-dynamic manipulator, that just uses inverse kinematics without using any physics engine functionality. The CoppeliaSim CAD data related to this tutorial (<em>redundantManipulator.stl</em>) is located in CoppeliaSim's installation folder <em>cadFiles</em>. A CoppeliaSim scene related to this tutorial can be found in <em>scenes/tutorials/InverseKinematics</em>. Click [Menu bar --&gt; File --&gt; Import --&gt; Mesh...] then select the file to import. Also refer to the section on how to <a href="importExport.htm">import/export shapes</a>. A dialog pops open with various import options. Click <em>Import</em>. A single <a href="shapes.htm">simple shape</a> was imported and is located in the middle of the scene. The shape also appears in the <a href="userInterface.htm#SceneHierarchy">scene hierarchy</a> on the left hand side of the main window. Depending on how the original CAD data was exported, the imported CAD data could be at a different scale, different location, or even subdivided into several shapes. Following figure shows the imported shape:<br>
</p>


<p align=center><img src="images/ikTut1.jpg"></p>
<br>

<p>As you can see, the import operation has left us with a single shape, where we expected several shapes. This means that we will have to divide the manipulator object ourselves: select the object (just click on it in the scene or the scene hierarchy), then click [Menu bar --&gt; Edit --&gt; Grouping/Merging --&gt; Divide selected shapes]. Following is what you should have:<br>
</p>

<p align=center><img src="images/ikTut2.jpg"></p>
<br>

<p>The original shape was divided into several sub-shapes (see also the scene hierarchy). The shape division algorithm operates by grouping all triangles that are linked by common edges. Depending on how the original mesh was created or exported, such a division procedure cannot be performed. In that case you will have to manually extract shapes in the <a href="triangleEditMode.htm">triangle edit mode</a> or in an external editor.<br>
</p>

<p>Next, we will change colors of the various objects so as to have a nice visual appearance. First double-click a shape icon in the scene hierarchy. The <a href="shapeProperties.htm">shape properties</a> dialog opens. While a shape is selected, click on <strong>Adjust  color</strong> in the dialog: this will allow you to adjust the various color components of the  selected shape. For now, just adjust the ambient/diffuse color component of your shapes. To transfer the color of one shape to another shape, select both shapes and make sure the last selected shape (indicated with a white bounding box) is the one you want to take the color from, then simply click the <strong>Apply to selection</strong> button in the <strong>Colors</strong> section of the shape dialog. Once you finished coloring, you might have following situation:<br>
</p>

<p align=center><img src="images/ikTut3.jpg"></p>
<br>

<p>In next step, we will add the 7 <a href="joints.htm">joints</a> of the manipulator. One way of doing this is to add the joints into the scene, then specify their appropriate position and orientation (through the <a href="positionDialog.htm">position dialog</a> and the <a href="orientationDialog.htm">orientation dialog</a>). This is however not possible, when you don't know the exact joint positions as in our case, and so we will have to <em>extract</em> them from the shapes that we have:<br>
</p>

<p>Select all imported shapes and click [Menu bar --&gt; Edit --&gt; Reorient bounding box --&gt; with reference frame of world]. This operation guarantees that our <a href="shapeReferenceFrames.htm">bounding boxes</a> are aligned with the absolute reference frame, and given the current manipulator configuration, represents the smallest bounding boxes. Click [Menu bar --&gt; Add --&gt; Joint --&gt; Revolute] to insert a revolute joint into the scene. The default position is at (0;0;0) and its default orientation is vertical, and so the joint is hidden by the manipulator's base cylinder. While the joint is still selected, ctrl-select the base cylinder, then open the <a href="positionDialog.htm">position dialog</a> on the <strong>position</strong> tab and click the <strong>Apply to selection</strong>. This just positioned the joint at the exact same coordinates as the base cylinder (this operation however only slightly adjusted the joint's vertical position since it was already almost in position). Now repeat the procedure for all other joints in the manipulator (remember there should be a total of 7). All joints are in position now, however, some of them have the wrong orientation. Select all joints that should be aligned with the world's Y-axis, then enter (90,0,0) for the <strong>Alpha</strong>, <strong>Beta</strong> and <strong>Gamma</strong> items in the <a href="orientationDialog.htm">orientation dialog</a>, on the <strong>orientation</strong> tab,  then click the <strong>Apply to selection</strong> button. Next, select the joint that should be aligned with the world's X-axis, then enter (0,90,0) for  <strong>Alpha</strong>, <strong>Beta</strong> and <strong>Gamma</strong>. All joints have the right position and orientation now. <br>
</p>

<p>You can now adjust the joint sizes (check the <strong>Length</strong> and <strong>Diameter</strong> items) in the <a href="jointProperties.htm">joint properties</a> dialog (that you can open by double-clicking a joint icon in the scene hierarchy). Make sure that all joints are clearly visible. This is what you should have:<br>
</p>

<p align=center><img src="images/ikTut5.jpg"></p>
<br>

<p>The next step in this tutorial is to group shapes that belong to the same rigid entity. Select the 5 shapes that are part of link 1 (the base cylinder being link 0), then click [Menu bar --&gt; Edit --&gt; Grouping/Merging --&gt; Group selected shapes]. Once the shapes are grouped in a compound shape, you could re-align its bounding box with the world, but this step is not required (and has only a visual effect). Repeat the same procedure with all shapes that logically belong together. In this tutorial we will not actuate the gripper's fingers, and so simply rigidly group them with the last link. When all shapes that are meant to be grouped share the same visual attributes, try merging them together instead: [Menu bar --&gt; Edit --&gt; Grouping/Merging --&gt; Merge selected shapes].<br>
</p>

<p>At this point you can rename all objects in the scene in following way, when going from base to tip: <em>redundantRobot</em> -  <em>redundantRob_joint1</em>  - <em>redundantRob_link1</em> - <em>redundantRob_joint2</em>, etc. Just double-click an object's alias in the scene hierarchy to edit it.<br>
</p>

<p>Now we can build the kinematic chain, going from tip to base: select object <em>redundantRob_link7</em>, then ctrl-select object <em>redundantRob_joint7</em> and click [Menu bar --&gt; Edit --&gt; Make last selected object parent]. Alternatively you can drag an object onto another one in the scene hierarchy to achieve a similar operation. Next do the same for object <em>redundantRob_joint7</em> and object <em>redundantRob_link6</em>. Continue in a same way until the whole kinematic chain of the manipulator was built. This is what you should have (notice the scene hierarchy structure):<br>
</p>

<p align=center><img src="images/ikTut6.jpg"></p>
<br>

<p>Select all joints, then in the joint dialog, select <strong>passive mode</strong>, then click <strong>Apply to selection</strong>. Keep the joints selected, then open the <a href="commonPropertiesDialog.htm">object common properties</a> and in the<strong> Visibility layers</strong> section, disable layer 2 and enable layer 10, then click the related <strong>Apply to selection</strong> button. This just sent all joints to the visibility layer 10, effectively making them invisible. Have a look at the <a href="layerSelectionDialog.htm">layer selection dialog</a> if you wish to temporarily enable/disable some layers.</p>
<p>In CoppeliaSim, an IK task requires specifying at least following elements:</p>
<li>a kinematic chain described with a <em>tip</em> dummy and a <em>base</em> object. </li>
<li>a <em>target</em> dummy that the <em>tip</em> dummy will be constrained to follow. </li>
<p>We already have the base object (object <em>redundantRobot</em>). Let's add a <a href="dummies.htm">dummy object</a>, rename it to <em>redundantRob_tip</em> and set its position to (0.324,0,0.62) using the coordinate and transformation dialog. Next, attach the dummy to <em>redundantRob_link7</em> (select <em>redundantRob_tip</em>, then <em>redundantRob_link7</em>, then [Menu bar --&gt; Edit --&gt; Make last selected object parent]. The <em>tip</em> dummy is ready!<br>
</p>
<p>Now let's prepare the <em>target</em> dummy: copy and paste <em>redundantRob_tip</em> and rename the copy to <em>redundantRob_target</em>. The <em>target</em> dummy is ready.</p>
<p>Now we will  add a way to easily manipulate the robot, without having to worry about breaking it by shifting the wrong objects around. We will therefore define it as a <a href="models.htm">model</a>. First, move <em>redundantRob_tip</em> and <em>redundantRob_target</em> to layer 11 to make both dummies invisible. Then shift-select all visible objects in the scene view, ctrl-click the object <em>redundantRobot</em> in the scene hierarchy to remove it from the selection, then open the <a href="commonPropertiesDialog.htm">object common properties</a> dialog. Check the <strong>Select base of model instead</strong> item, then the related <strong>Apply to selection</strong> button. Clear the selection with &lt;ESC&gt;, then select <em>redundantRobot</em>. In the same dialog, check the <strong>Object is model base</strong> item, then close the dialogs. Notice how a stippled bounding box now encompasses the whole manipulator:<br>
</p>
<p align=center><img src="images/ikTut9.jpg"></p>
<br>
<p>Click any object on the manipulator and notice how the base object <em>redundantRobot</em> always gets selected instead.<br>
</p>
<p>Next, we add a <em>manipulation sphere</em> that we will use to manipulate the robot's gripper position/orientation. Click [Menu bar --&gt; Add --&gt; Primitive shape --&gt; Sphere] to open the <a href="primitiveShapes.htm">primitive shape dialog</a>, indicate 0.05 for the <strong>X-Size</strong>, <strong>Y-Size</strong> and <strong>Z-Size</strong>, then uncheck the <strong>Create dynamic and respondable shape</strong> item and click <strong>OK</strong>. Adjust the newly added sphere's position to be the same as <em>redundantRob_target</em> (using the coordinate and transformation dialog). The sphere now appears at the tip of the manipulator. Rename the sphere to <em>redundantRob_manipSphere</em>, then make it parent of <em>redundantRob_target</em>. Make <em> redundantRobot</em> parent of <em>rendundantRob_manipSphere</em>: the target dummy and the manipulation sphere are now also part of the robot model. Collapse the <em>redundantRobot</em> tree in the scene hierarchy. The redundant manipulator model is ready!</p>


<br>
<br>
<table class=subsectionTable><tr class=subsectionTd><td class=subsectionTd>
<a name="setup"></a>Setting up the inverse kinematics task
</td></tr></table>




<p>Inverse kinematics is entirely set up by having a script calling appropriate API commands: the idea is to build an equivalent kinematic model via the  functions provided by the <a href="kinematicsPlugin.htm">kinematics plugin</a>. The approach uses the concept and terminology of <a href="basicsOnIkGroupsAndIkElements.htm">IK groups and IK elements</a>.</p>
<p>Select object <em>redundantRobot</em>, then [Menu bar --&gt; Add --&gt; Associated child script --&gt; Non-threaded] to attach a <a href="childScripts.htm">child script</a> to that object. Double-click the script icon that appeared next to the object alias, and replace the script content with following code:</p>

<pre class=lightRedBox>function sysCall_init()
    -- Build a kinematic chain and 2 IK groups (undamped and damped) inside of the IK plugin environment,
    -- based on the kinematics of the robot in the scene:
    -- There is a simple way, and a more elaborate way (but which gives you more options/flexibility):

    -- Simple way:
    local simBase=sim.getObjectHandle('.')
    local simTip=sim.getObjectHandle('./redundantRob_tip')
    local simTarget=sim.getObjectHandle('./redundantRob_target')
    -- create an IK environment:
    ikEnv=simIK.createEnvironment()
    -- create an IK group: 
    ikGroup_undamped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to undamped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_undamped,simIK.method_pseudo_inverse,0,6)
    -- create an IK element based on the scene content: 
    simIK.addIkElementFromScene(ikEnv,ikGroup_undamped,simBase,simTip,simTarget,simIK.constraint_pose)
    -- create another IK group: 
    ikGroup_damped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to damped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_damped,simIK.method_damped_least_squares,1,99)
    -- create an IK element based on the scene content: 
    simIK.addIkElementFromScene(ikEnv,ikGroup_damped,simBase,simTip,simTarget,simIK.constraint_pose) 
    
    -- Elaborate way:
    --[[
    simBase=sim.getObjectHandle('.')
    simTip=sim.getObjectHandle('./redundantRob_tip')
    simTarget=sim.getObjectHandle('./redundantRob_target')
    simJoints={}
    for i=1,7,1 do
        simJoints[i]=sim.getObjectHandle('./redundantRob_joint'..i)
    end
    ikJoints={}
    -- create an IK environment:
    ikEnv=simIK.createEnvironment()
    -- create a dummy in the IK environment: 
    ikBase=simIK.createDummy(ikEnv)
    -- set that dummy into the same pose as its CoppeliaSim counterpart: 
    simIK.setObjectMatrix(ikEnv,ikBase,-1,sim.getObjectMatrix(simBase,-1)) 
    local parent=ikBase
    for i=1,#simJoints,1 do -- loop through all joints
        -- create a joint in the IK environment:
        ikJoints[i]=simIK.createJoint(ikEnv,simIK.jointtype_revolute)
        -- set it into IK mode: 
        simIK.setJointMode(ikEnv,ikJoints[i],simIK.jointmode_ik)
        -- set the same joint limits as its CoppeliaSim counterpart joint: 
        local cyclic,interv=sim.getJointInterval(simJoints[i])
        simIK.setJointInterval(ikEnv,ikJoints[i],cyclic,interv)
        -- set the same joint position as its CoppeliaSim counterpart joint: 
        simIK.setJointPosition(ikEnv,ikJoints[i],sim.getJointPosition(simJoints[i]))
        -- set the same object pose as its CoppeliaSim counterpart joint: 
        simIK.setObjectMatrix(ikEnv,ikJoints[i],-1,sim.getObjectMatrix(simJoints[i],-1))
        -- set its corresponding parent: 
        simIK.setObjectParent(ikEnv,ikJoints[i],parent,true) 
        parent=ikJoints[i]
    end
    -- create the tip dummy in the IK environment:
    ikTip=simIK.createDummy(ikEnv)
    -- set that dummy into the same pose as its CoppeliaSim counterpart: 
    simIK.setObjectMatrix(ikEnv,ikTip,-1,sim.getObjectMatrix(simTip,-1))
    -- attach it to the kinematic chain: 
    simIK.setObjectParent(ikEnv,ikTip,parent,true)
    -- create the target dummy in the IK environment: 
    ikTarget=simIK.createDummy(ikEnv)
    -- set that dummy into the same pose as its CoppeliaSim counterpart: 
    simIK.setObjectMatrix(ikEnv,ikTarget,-1,sim.getObjectMatrix(simTarget,-1))
    -- link the two dummies: 
    simIK.setLinkedDummy(ikEnv,ikTip,ikTarget)
    -- create an IK group: 
    ikGroup_undamped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to undamped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_undamped,simIK.method_pseudo_inverse,0,6)
    -- make sure the robot doesn't shake if the target position/orientation wasn't reached: 
    simIK.setIkGroupFlags(ikEnv,ikGroup_undamped,1+2+4+8)
    -- add an IK element to that IK group: 
    local ikElementHandle=simIK.addIkElement(ikEnv,ikGroup_undamped,ikTip)
    -- specify the base of that IK element: 
    simIK.setIkElementBase(ikEnv,ikGroup_undamped,ikElementHandle,ikBase)
    -- specify the constraints of that IK element: 
    simIK.setIkElementConstraints(ikEnv,ikGroup_undamped,ikElementHandle,simIK.constraint_pose)
    -- create another IK group: 
    ikGroup_damped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to damped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_damped,simIK.method_damped_least_squares,1,99)
    -- add an IK element to that IK group: 
    local ikElementHandle=simIK.addIkElement(ikEnv,ikGroup_damped,ikTip)
    -- specify the base of that IK element: 
    simIK.setIkElementBase(ikEnv,ikGroup_damped,ikElementHandle,ikBase)
    -- specify the constraints of that IK element: 
    simIK.setIkElementConstraints(ikEnv,ikGroup_damped,ikElementHandle,simIK.constraint_pose) 
    --]]
end

function sysCall_actuation()
    -- There is a simple way, and a more elaborate way (but which gives you more options/flexibility):
    
    -- Simple way:
    -- try to solve with the undamped method:
    if simIK.applyIkEnvironmentToScene(ikEnv,ikGroup_undamped,true)==simIK.result_fail then 
        -- the position/orientation could not be reached.
        -- try to solve with the damped method:
        simIK.applyIkEnvironmentToScene(ikEnv,ikGroup_damped)
        -- We display a IK failure report message: 
        if not ikFailedReportHandle then 
            ikFailedReportHandle=sim.displayDialog("IK failure report","IK solver failed.",
                sim.dlgstyle_message,false,"",nil,{1,0.7,0,0,0,0})
        end
    else
        if ikFailedReportHandle then
        	-- We close any report message about IK failure:
            sim.endDialog(ikFailedReportHandle) 
            ikFailedReportHandle=nil
        end
    end
    
    -- Elaborate way:
    --[[
    -- reflect the pose of the target dummy to its counterpart in the IK environment:
    simIK.setObjectMatrix(ikEnv,ikTarget,ikBase,sim.getObjectMatrix(simTarget,simBase)) 
	-- try to solve with the undamped method:
    if simIK.handleIkGroup(ikEnv,ikGroup_undamped)==simIK.result_fail then 
        -- the position/orientation could not be reached.
        -- try to solve with the damped method:
        simIK.handleIkGroup(ikEnv,ikGroup_damped)
        -- We display a IK failure report message: 
        if not ikFailedReportHandle then 
            ikFailedReportHandle=sim.displayDialog("IK failure report","IK solver failed.",
                sim.dlgstyle_message,false,"",nil,{1,0.7,0,0,0,0})
        end
    else
        if ikFailedReportHandle then
        	-- We close any report message about IK failure:
            sim.endDialog(ikFailedReportHandle) 
            ikFailedReportHandle=nil
        end
    end
    -- apply the joint values computed in the IK environment to their CoppeliaSim joint counterparts:
    for i=1,#simJoints,1 do
        sim.setJointPosition(simJoints[i],simIK.getJointPosition(ikEnv,ikJoints[i])) 
    end
    --]]
end 

function sysCall_cleanup()
	-- erase the IK environment: 
    simIK.eraseEnvironment(ikEnv) 
end </pre>




<p>Above script creates an equivalent kinematic model from the CoppeliaSim model, then in each simulation step, reads the position/orientation of the CoppeliaSim target, applies it to the target of the equivalent kinematic model, runs the IK solver, and finally reads the joint angles of the equivalent kinematic model, and applies them to the joints of the CoppeliaSim model. To handle singular configurations and situations where the target is out of reach, we first try with a non-damped solver, and if it fails, we revert to a damped solver (with a damped solver, when damping is large, resolution becomes more stable but convergence slower).</p>

<br>
<table class=subsectionTable>
  <tr class=subsectionTd><td class=subsectionTd>
<a name="run"></a>Running the simulation
</td></tr></table>


<p>Our inverse kinematics task is ready! Let's test it. Run the simulation, then select the green manipulation sphere. Next,  select the <a href="positionDialog.htm">object translation</a> toolbar button:<br>
</p>
<p align=center><img src="images/objectShiftButton.jpg"></p>
<br>

<p>Now drag the object with the mouse: the manipulator should follow. Also try the <a href="orientationDialog.htm">object rotation</a> toolbar button:<br>
</p>

<p align=center><img src="images/objectRotateButton.jpg"></p>
<br>


<p>Try also holding down the ctr- or shift-keys during manipulation. Switch back to the object translation toolbar button, and try to drag the object as far as possible, and notice how the inverse kinematics task is quite robust, thanks to the damped component. Stop the simulation, then disable the damped IK group and try again. Try also to disable individual constraints in the corresponding IK element and notice how the manipulator behaves during simulation.<br>
</p>

<p>Run the simulation, and copy-paste a few times the manipulator and shift/rotate the copies around, also changing their configurations by dragging their manipulation spheres. Notice how every manipulator instance is fully functional regarding IK.<br>
</p>


</tr>
</table> 
</div>  
  
  
</body>

</html>
